#coding=utf-8
'''
    机器人理论-物理层面的数据定义
'''
import numpy as np

import app
from controller.geometric import *
from controller.params import Params
from interfaces.udp import udp_data_bridge
from definations.definations import *
import copy
from controller import pid
class Physical:
    '''
    模型与物理之间的映射装置
    '''
    def __init__(self,rootObject):
        '''

        :param base:
        :param lFoot:
        :param rFoot:
        :param lChain:
        :param rChain:
        '''
        '''
            还原真实物理状态，构建一个模型化的shadow
        '''
        #
        self.rootObject=rootObject
        self.__lFoot=copy.deepcopy(rootObject.lFoot)
        self.__rFoot=copy.deepcopy(rootObject.rFoot)
        self.__base=copy.deepcopy(rootObject.base)
        self.__lChain=copy.deepcopy(rootObject.lChain)
        self.__rChain=copy.deepcopy(rootObject.rChain)
        #   实体关节描述
        self.__lChain.joints = self.rootObject.lChain.joints.copy()
        self.__lChain.terminal = self.__lFoot
        self.__lChain.base = self.__base
        self.__rChain.joints = self.rootObject.rChain.joints.copy()
        self.__rChain.terminal = self.__rFoot
        self.__rChain.base = self.__base

        #   拾取Free和Support
        self.freeChain=self.rootObject.lChain
        self.freeFoot=self.rootObject.lFoot
        self.supportChain=self.rootObject.lChain
        self.supportFoot=self.rootObject.lFoot
        self.__freeChain = self.__lChain
        self.__freeFoot = self.__lFoot
        self.__supportChain = self.__lChain
        self.__supportFoot = self.__lFoot

        #   配置信息
        #   重心
        self.massCenter=geolib.GeoEntity()
    def Update(self):
        '''
        更新所有状态信息
        :return:
        '''
        self.BuildPhysical()
        self.UpdateError()
        self.UpdateMassCenter()
    def BuildPhysical(self):
        '''
        构建真实的机器人运动状态
        :return:
        self.__*Foot.error: 局部误差（修正误差）
        self.*Foot.error: 全局误差（整体误差）
        '''
        self.__lChain.joints=self.rootObject.lChain.joints.copy()
        self.__rChain.joints=self.rootObject.rChain.joints.copy()
        if self.supportFoot==None:
            '''
                悬空状态
            '''
            self.__base.SetPose(self.rootObject.base.VectorPose())
            self.__base.SetRot(self.rootObject.base.rotationMonitored)
            self.__lChain.BuildChain(autoAdjust='terminal')
            self.__rChain.BuildChain(autoAdjust='terminal')
            for i in range(2):
                __theoTerminal=geolib.GeoEntity()
                __theoTerminal.Mimic(self.__freeFoot[i])
                self.__freeFoot[i].SetRot(self.freeFoot[i].rotationMonitored)
                x,y,z,roll,pitch,yaw=self.__freeFoot[i].LookupTransform(__theoTerminal)
                self.__freeFoot[i].error.SetGeoData([x,y,z],[roll,pitch,yaw])
            return

        #   更新误差
        self.__supportFoot.Mimic(self.supportFoot)
        self.__supportFoot.SetRot(self.supportFoot.rotationMonitored)
        x,y,z,roll,pitch,yaw=self.supportFoot.LookupTransform(self.__supportFoot)
        self.__supportFoot.error.SetGeoData([x,y,z],[roll,pitch,yaw])       #   模型误差
        self.supportFoot.error.SetGeoData([x, y, z], [roll, pitch, yaw])    #   实际物理误差

        '''构建实体几何关节'''

        self.__supportChain.BuildChain(autoAdjust='base')   #构建出base
        __theoBase=geolib.GeoEntity()
        __theoBase.Mimic(self.__base)
        self.__base.SetRot(self.rootObject.base.rotationMonitored)
        x,y,z,roll,pitch,yaw=__theoBase.LookupTransform(self.__base)
        self.__base.error.SetGeoData([x,y,z],[roll,pitch,yaw])

        x, y, z, roll, pitch, yaw = self.rootObject.base.LookupTransform(self.__base)
        self.rootObject.base.error.SetGeoData([x, y, z], [roll, pitch, yaw])


        self.__freeChain.BuildChain(autoAdjust='terminal')
        __theoBase = geolib.GeoEntity()
        __theoBase.Mimic(self.__freeFoot)
        self.__freeFoot.SetRot(self.freeFoot.rotationMonitored)
        x, y, z, roll, pitch, yaw = __theoBase.LookupTransform(self.__freeFoot)
        self.__freeFoot.error.SetGeoData([x,y,z],[roll,pitch,yaw])

        x, y, z, roll, pitch, yaw = self.freeFoot.LookupTransform(self.__freeFoot)
        self.freeFoot.error.SetGeoData([x,y,z],[roll,pitch,yaw])

    def UpdateSupportState(self):
        '''
        通过Terminal压力数据更新机器人的支撑状态
        :return: 是否发生变换
        '''
        pT=(self.rootObject.lFoot.pressureTotal+self.rootObject.rFoot.pressureTotal)/2    #总压力
        #   计算每一只脚承担的压力数值
        if pT<=0:
            self.freeChain=[self.rootObject.lChain,self.rootObject.rChain]
            self.freeFoot=[self.rootObject.lFoot,self.rootObject.rFoot]
            self.supportChain = None
            self.supportFoot=None
            self.__freeChain=[self.__lChain,self.__rChain]
            self.__freeFoot=[self.__lFoot,self.__rFoot]
            self.__supportFoot=None
            self.__supportChain=None
            return

        for ft in [self.rootObject.lFoot,self.rootObject.rFoot]:
            ft.supportRatio=ft.pressureTotal/pT
        if self.rootObject.lFoot.supportRatio<self.rootObject.rFoot.supportRatio:
            self.freeChain=self.rootObject.lChain
            self.freeFoot=self.rootObject.lFoot
            self.__freeChain=self.__lChain
            self.__freeFoot=self.__lFoot
            self.supportFoot=self.rootObject.rFoot
            self.supportChain=self.rootObject.rChain
            self.__supportFoot=self.__rFoot
            self.__supportChain=self.__rChain
        else:
            self.freeChain = self.rootObject.rChain
            self.freeFoot = self.rootObject.rFoot
            self.__freeChain = self.__rChain
            self.__freeFoot = self.__rFoot
            self.supportFoot = self.rootObject.lFoot
            self.supportChain = self.rootObject.lChain
            self.__supportFoot = self.__lFoot
            self.__supportChain = self.__lChain

    def UpdateError(self):
        '''
            更新物理学误差
        :return:
        '''
        for terminal in [self.__base,self.__lFoot,self.__rFoot]:
            terminal.error.DumpCache()

        if self.supportFoot!=None:
            #更新roll误差
            self.supportChain.jErrorDynamic[4]+=pid.PID(self.__base.error.rotQueue[-1][0],self.__base.error.rotQueue[-2][0],
                                                        p=Params.controlParams['physical']['pid_x_p'],
                                                        i=Params.controlParams['physical']['pid_x_i'],
                                                        d=Params.controlParams['physical']['pid_x_d'])
            self.freeChain.jErrorDynamic[4]*=Params.controlParams['physical']['decay_ratio']
            #更新Pitch误差
            self.supportChain.jErrorDynamic[3]+=pid.PID(self.__base.error.rotQueue[-1][1],self.__base.error.rotQueue[-2][1],p=0.022,i=0.0,d=0.6)
            self.freeChain.jErrorDynamic[3] = self.supportChain.jErrorDynamic[3]
        else:
            #悬空状态
            pass
        self.rootObject.lChain.jErrorDynamic=np.clip(self.rootObject.lChain.jErrorDynamic,a_min=-Params.controlParams['physical']['joint_error_limit'],a_max=Params.controlParams['physical']['joint_error_limit'])
        self.rootObject.rChain.jErrorDynamic = np.clip(self.rootObject.rChain.jErrorDynamic,a_min=-Params.controlParams['physical']['joint_error_limit'],a_max=Params.controlParams['physical']['joint_error_limit'])
        #   更新过程

    def GetFoots(self):
        return [self.__lFoot, self.__rFoot]

    def GetChains(self):
        return [self.__lChain, self.__rChain]

    def GetBase(self):
        return self.__base

    def GetSupportChain(self):
        return self.__supportChain
    def GetFreeChain(self):
        return self.__freeChain
    def GetFreeFoot(self):
        return self.__freeFoot
    def GetSupportFoot(self):
        return self.__supportFoot

    def UpdateMassCenter(self):
        '''
        更新重心
        :return:
        '''
        c = np.zeros(3)
        mass = 0.0
        c += self.__lChain.massCenter * self.__lChain.mass
        mass += self.__lChain.mass
        c += self.__rChain.massCenter * self.__rChain.mass
        mass += self.__rChain.mass
        c += (self.__base.VectorPose() + self.__base.gcBias) * self.__base.weight
        mass += self.__base.weight
        c /= mass
        self.massCenter = geolib.GeoEntity(c[0], c[1], c[2])

    def Sync(self):
        if self.rootObject.panel.ui_Main.autoCheck.isChecked():
            l=np.array(self.rootObject.lChain.jErrorStatic)+np.array(self.rootObject.lChain.joints)+np.array(self.rootObject.lChain.jErrorDynamic)
            r=np.array(self.rootObject.rChain.jErrorStatic)+np.array(self.rootObject.rChain.joints)+np.array(self.rootObject.rChain.jErrorDynamic)
            h=0.0
            self.rootObject.netBridge.SyncJoint(list(l),list(r),h,1.0/Params.controlParams['freq_med'])